#include <rclcpp/rclcpp.hpp>
#include <my_custom_msgs/msg/time_info.hpp>  // 添加自定义消息头文件
#include <std_msgs/msg/string.hpp>
#include <QApplication>
#include <QWidget>
#include <QLabel>
#include <QPushButton>
#include <QTimer>
#include <QPixmap>
#include <QMovie>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QMessageBox>
#include <QProcess>
#include <QDebug>
#include "path_plan/cartesian_path.h" 
#include <thread>

bool executeCartesianPath1(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger);
bool executeCartesianPath2(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger);
bool executeCartesianPath3(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger);

class ControlPanel : public QWidget {
    Q_OBJECT

public:
    ControlPanel(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr) : QWidget(parent), node_(node), move_group(node, "zc4") {
        setWindowTitle("助餐机器人控制面板");
        setFixedSize(500, 400);

        // 设置主布局
        QVBoxLayout* mainLayout = new QVBoxLayout(this);

        // 上方时间显示
        QHBoxLayout* topLayout = new QHBoxLayout();
        timeLabel = new QLabel("当前时间: 未接收到\n运行时间: 未接收到", this);
        topLayout->addWidget(timeLabel);
        mainLayout->addLayout(topLayout);

        QHBoxLayout* statusLayout = new QHBoxLayout();
        QVBoxLayout* INFOLayout = new QVBoxLayout();
        // 当前状态标签
        statusLabel = new QLabel("当前状态: 空闲", this);
        statusLabel->setStyleSheet("border: 1px solid black; font-size: 22px; padding: 10px;");
        statusLabel->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);  // 自适应宽度，高度固定
        statusLabel->setMaximumWidth(220);  // 设置最大宽度，避免过长
        INFOLayout->addWidget(statusLabel);

        // 取食路径标签
        pathLabel = new QLabel("取食路径: 无", this);
        pathLabel->setStyleSheet("border: 1px solid black; font-size: 22px; padding: 10px;");
        pathLabel->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);  // 自适应宽度，高度固定
        pathLabel->setMaximumWidth(220);  // 设置最大宽度，避免过长
        INFOLayout->addWidget(pathLabel);

        // 喂食速度标签
        pathSetLabel = new QLabel("喂食速度: 中", this);
        pathSetLabel->setStyleSheet("border: 1px solid black; font-size: 22px; padding: 10px;");
        pathSetLabel->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);  // 自适应宽度，高度固定
        pathSetLabel->setMaximumWidth(220);  // 设置最大宽度，避免过长
        INFOLayout->addWidget(pathSetLabel);

        statusLayout->addLayout(INFOLayout);

        // GIF显示区域
        gifLabel = new QLabel(this);
        gifLabel->setFixedSize(260, 200);
        gifLabel->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
        statusLayout->addWidget(gifLabel);

        mainLayout->addLayout(statusLayout);

        // 控制按钮布局
        QVBoxLayout* buttonLayout = new QVBoxLayout();
        
        QHBoxLayout* controlButtonLayout = new QHBoxLayout();
        forceButton = new QPushButton("力控", this);
        visualButton = new QPushButton("视觉", this);
        homeButton = new QPushButton("HOME", this);
        processButton = new QPushButton("流程", this);
        resetButton = new QPushButton("reset", this);

        forceButton->setStyleSheet("font-size: 20px;");
        forceButton->setFixedSize(100, 50);
        visualButton->setStyleSheet("font-size: 20px;");
        visualButton->setFixedSize(100, 50);
        homeButton->setStyleSheet("font-size: 20px;");
        homeButton->setFixedSize(100, 50);
        processButton->setStyleSheet("font-size: 20px;");
        processButton->setFixedSize(100, 50);
        resetButton->setStyleSheet("font-size: 20px;");
        resetButton->setFixedSize(100, 50);

        controlButtonLayout->addWidget(forceButton);
        controlButtonLayout->addWidget(visualButton);
        controlButtonLayout->addWidget(homeButton);
        controlButtonLayout->addWidget(processButton);
        controlButtonLayout->addWidget(resetButton);
        buttonLayout->addLayout(controlButtonLayout);

        // 取食路径选择按钮
        QHBoxLayout* pathButtonLayout = new QHBoxLayout();
        pathButton1 = new QPushButton("取食路径一", this);
        pathButton2 = new QPushButton("取食路径二", this);
        pathButton3 = new QPushButton("取食路径三", this);

        pathButton1->setStyleSheet("font-size: 20px;");
        pathButton1->setFixedSize(150, 50);
        pathButton2->setStyleSheet("font-size: 20px;");
        pathButton2->setFixedSize(150, 50);
        pathButton3->setStyleSheet("font-size: 20px;");
        pathButton3->setFixedSize(150, 50);

        pathButtonLayout->addWidget(pathButton1);
        pathButtonLayout->addWidget(pathButton2);
        pathButtonLayout->addWidget(pathButton3);
        buttonLayout->addLayout(pathButtonLayout);

        mainLayout->addLayout(buttonLayout);

        // 信号和槽连接
        connect(forceButton, &QPushButton::clicked, this, &ControlPanel::executeForceControl);
        connect(visualButton, &QPushButton::clicked, this, &ControlPanel::executeVisualControl);
        connect(homeButton, &QPushButton::clicked, this, &ControlPanel::killProcess);
        connect(processButton, &QPushButton::clicked, this, &ControlPanel::executeAllProcess);
        connect(resetButton, &QPushButton::clicked, this, &ControlPanel::restartProgram);
        connect(pathButton1, &QPushButton::clicked, this, &ControlPanel::executeTrajectory1);
        connect(pathButton2, &QPushButton::clicked, this, &ControlPanel::executeTrajectory2);
        connect(pathButton3, &QPushButton::clicked, this, &ControlPanel::executeTrajectory3);

        // 添加ROS 2订阅器
        subscriber_ = node_->create_subscription<my_custom_msgs::msg::TimeInfo>(
            "time_info", 10, std::bind(&ControlPanel::timeCallback, this, std::placeholders::_1));
        RCLCPP_INFO(node_->get_logger(), "Subscribed to /time_info");

        // 设置ROS 2事件处理定时器
        rosTimer = new QTimer(this);
        connect(rosTimer, &QTimer::timeout, [this]() {
            rclcpp::spin_some(node_);  // 定期处理ROS 2事件
        });
        rosTimer->start(100);  // 每100ms处理一次ROS 2事件

        // 设置ROS 2发布器
        publisher_ = node_->create_publisher<std_msgs::msg::String>("control_panel", 10);

        // 初始化显示空闲状态的图片
        showImage("/home/jetson/图片/home.jpeg");
    }

private slots:
    void executeForceControl() {
        publishMessage("执行力控操作");
        QMessageBox::information(this, "Info", "执行力控操作");
    }

    void executeVisualControl() {
        publishMessage("执行视觉操作");
        QMessageBox::information(this, "Info", "执行视觉操作");
    }

    void killProcess() {
        publishMessage("重置进程");
        QMessageBox::information(this, "Info", "重置进程");
    }

    void executeAllProcess() {
        publishMessage("执行所有进程");
        QMessageBox::information(this, "Info", "执行所有进程");
    }

    void restartProgram() {
        QApplication::quit();
        QProcess::startDetached(qApp->arguments()[0], qApp->arguments());
    }

    void executeTrajectory1() {
        // 立即更新状态和路径信息
        statusLabel->setText("当前状态: 工作");
        pathLabel->setText("取食路径: 一");
        publishMessage("执行路径一");
        showGif("/home/jetson/图片/轨迹1.gif");
    
        // 创建一个新的线程来执行路径规划
        std::thread pathThread([this]() {
            // 执行路径规划
            executeCartesianPath1(move_group, node_->get_logger());
    
            // 路径规划完成后，更新 UI (通过主线程)
            QMetaObject::invokeMethod(this, [&]() {
                // 更新状态和路径信息
                statusLabel->setText("当前状态: 空闲");
                pathLabel->setText("取食路径: 无");
                showImage("/home/jetson/图片/home.jpeg");
            });
        });
    
        // 在主线程中继续执行其他操作
        pathThread.detach();  // 让路径规划线程在后台运行
    }
    

    void executeTrajectory2() {
        // 立即更新状态和路径信息
        statusLabel->setText("当前状态: 工作");
        pathLabel->setText("取食路径: 二");
        publishMessage("执行路径二");
        showGif("/home/jetson/图片/轨迹2.gif");
    
        // 创建一个新的线程来执行路径规划
        std::thread pathThread([this]() {
            // 执行路径规划
            executeCartesianPath2(move_group, node_->get_logger());
    
            // 路径规划完成后，更新 UI (通过主线程)
            QMetaObject::invokeMethod(this, [&]() {
                // 更新状态和路径信息
                statusLabel->setText("当前状态: 空闲");
                pathLabel->setText("取食路径: 无");
                showImage("/home/jetson/图片/home.jpeg");
            });
        });
    
        // 在主线程中继续执行其他操作
        pathThread.detach();  // 让路径规划线程在后台运行
    }
    

    void executeTrajectory3() {
        // 立即更新状态和路径信息
        statusLabel->setText("当前状态: 工作");
        pathLabel->setText("取食路径:三 ");
        publishMessage("执行路径三");
        showGif("/home/jetson/图片/轨迹3.gif");
    
        // 创建一个新的线程来执行路径规划
        std::thread pathThread([this]() {
            // 执行路径规划
            executeCartesianPath3(move_group, node_->get_logger());
    
            // 路径规划完成后，更新 UI (通过主线程)
            QMetaObject::invokeMethod(this, [&]() {
                // 更新状态和路径信息
                statusLabel->setText("当前状态: 空闲");
                pathLabel->setText("取食路径: 无");
                showImage("/home/jetson/图片/home.jpeg");
            });
        });
    
        // 在主线程中继续执行其他操作
        pathThread.detach();  // 让路径规划线程在后台运行
    }

    void showGif(const QString& filePath) {
        QMovie* movie = new QMovie(filePath);
        gifLabel->setMovie(movie);
        QSize labelSize = gifLabel->size(); // 获取 gifLabel 的当前尺寸，设置 GIF 动画的缩放大小
        movie->setScaledSize(labelSize);  // 让 GIF 动画缩放到标签的大小
        movie->start();
    }

    void showImage(const QString& filePath) {
        QPixmap pixmap(filePath);
        // 缩放图片以适应标签大小
        QPixmap scaledPixmap = pixmap.scaled(gifLabel->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation);
        gifLabel->setPixmap(scaledPixmap);
        gifLabel->setAlignment(Qt::AlignCenter);
    }

    void publishMessage(const QString& message) {
        auto msg = std_msgs::msg::String();
        msg.data = message.toStdString();
        RCLCPP_INFO(node_->get_logger(), "Publishing: '%s'", msg.data.c_str());
        publisher_->publish(msg);
    }

    // 定义回调函数处理时间消息
    void timeCallback(const my_custom_msgs::msg::TimeInfo::SharedPtr msg) {
        // RCLCPP_INFO(node_->get_logger(), "Received time info: %s", msg->current_time.c_str());
        // 使用invokeMethod确保UI更新在主线程中执行
        QMetaObject::invokeMethod(this, [this, msg]() {
            timeLabel->setText(
                QString("当前时间: %1\n运行时间: %2")
                    .arg(QString::fromStdString(msg->current_time))
                    .arg(QString::fromStdString(msg->elapsed_time))
            );
        });
    }

private:
    rclcpp::Node::SharedPtr node_;
    moveit::planning_interface::MoveGroupInterface move_group;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::Subscription<my_custom_msgs::msg::TimeInfo>::SharedPtr subscriber_;  // 新增订阅器
    QLabel* timeLabel;
    QLabel* statusLabel;
    QLabel* pathLabel;
    QLabel* pathSetLabel;
    QPushButton* forceButton;
    QPushButton* visualButton;
    QPushButton* homeButton;
    QPushButton* processButton;
    QPushButton* resetButton;
    QPushButton* pathButton1;
    QPushButton* pathButton2;
    QPushButton* pathButton3;
    QLabel* gifLabel;
    QTimer* rosTimer;  // 新增ROS 2事件处理定时器
};

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("qt_ros2_control_panel");
    QApplication app(argc, argv);
    ControlPanel panel(node);
    panel.show();
    return app.exec();
}

#include "interface_test.moc"
